#ifndef MATRIX
#define MATRIX

#include <stdio.h>
#include <memory.h>
#include <algorithm>

#include <base\vec3f.h>
#include <base\rotation.h>

typedef float mat[4][4];

const mat IDENTITYMATRIX = {
  { 1.0f, 0.0f, 0.0f, 0.0f },
  { 0.0f, 1.0f, 0.0f, 0.0f },
  { 0.0f, 0.0f, 1.0f, 0.0f },
  { 0.0f, 0.0f, 0.0f, 1.0f }
};

class matrixp {
public:

  static bool isIdentity(const float fm[][4]) {
    return (fm[0][0]==1.0f) && memcmp(&fm[0][1], &IDENTITYMATRIX[0][1], (4 * 3 + 3) * sizeof(float)) == 0;
  }
};

class matrix {

  float matrix_[4][4];

public:

  matrix(void) {
  }

  matrix &multRight(const matrix & m) {
  
    const mat & mfm = m.matrix_;
    if (matrixp::isIdentity(mfm)) { 
      return *this; 
    }

    mat & tfm = matrix_;
    if (matrixp::isIdentity(tfm)) { 
      *this = m; 
      return *this; 
    }

    mat tmp;
    (void)memcpy(tmp, tfm, 4*4*sizeof(float));

    for (int i=0; i < 4; i++) {
      for (int j=0; j < 4; j++) {
        tfm[i][j] =
          tmp[i][0] * mfm[0][j] +
          tmp[i][1] * mfm[1][j] +
          tmp[i][2] * mfm[2][j] +
          tmp[i][3] * mfm[3][j];
      }
    }

    return *this;
  }

  void makeIdentity(void) {
    matrix_[0][0] = matrix_[1][1] =
    matrix_[2][2] = matrix_[3][3] = 1.0f;

    matrix_[0][1] = matrix_[0][2] = matrix_[0][3] =
    matrix_[1][0] = matrix_[1][2] = matrix_[1][3] =
    matrix_[2][0] = matrix_[2][1] = matrix_[2][3] =
    matrix_[3][0] = matrix_[3][1] = matrix_[3][2] = 0.0f;
  }

  void setRotate(const rotation & q) {
    q.getValue(*this);
  }

  void setTranslate(const vec3f & t) {
    this->makeIdentity();
    matrix_[3][0] = t.x;
    matrix_[3][1] = t.y;
    matrix_[3][2] = t.z;
  }

  void setTransform(const vec3f & t, const rotation & r) {
    matrix tmp;
    const rotation identity = rotation::identity();

    this->setRotate(r);
    if ((vec3f) t != vec3f(0.f, 0.0f, 0.0f)) {
      tmp.setTranslate(t);
      this->multRight(tmp);
    }
  }

  void multVecMatrix(const vec3f & src, vec3f & dst) const {
    // Checks if the "this" matrix is equal to the identity matrix.  See
    // also code comments at the start of SbMatrix::multRight().
    if (matrixp::isIdentity(this->matrix_)) { dst = src; return; }

    const float * t0 = this->matrix_[0];
    const float * t1 = this->matrix_[1];
    const float * t2 = this->matrix_[2];
    const float * t3 = this->matrix_[3];

    // Copy the src vector, just in case src and dst is the same vector.
    vec3f s = src;

    float W = s[0]*t0[3] + s[1]*t1[3] + s[2]*t2[3] + t3[3];

    dst[0] = (s[0]*t0[0] + s[1]*t1[0] + s[2]*t2[0] + t3[0])/W;
    dst[1] = (s[0]*t0[1] + s[1]*t1[1] + s[2]*t2[1] + t3[1])/W;
    dst[2] = (s[0]*t0[2] + s[1]*t1[2] + s[2]*t2[2] + t3[2])/W;
  }

  float * operator [](int i) {
    return this->matrix_[i];
  }

  const float *operator [](int i) const {
    return this->matrix_[i];
  }
};

#endif